#include <ros/ros.h>
#include <sstream>
#include "prometheus_gimbal_control/GimbalState.h"
#include "prometheus_gimbal_control/VisionDiff.h"
#include "prometheus_msgs/ControlCommand.h"
#include "prometheus_msgs/DroneState.h"
#include "printf_utils.h"

float curr_pos[3];
std::string curr_mode;
// 惯性系
void droneStateCb(const prometheus_msgs::DroneState::ConstPtr &msg)
{
    // curr_pos[2] = msg->position[2]
    curr_pos[2] = msg->rel_alt;
    curr_mode = msg->mode;
}

int flag_detected = 0; // 是否检测到目标标志
float g_curr_size = 0;
int g_past_bbox_id = 0;
int g_curr_bbox_id = 0;
const int g_mean_lenght = 10;
static float g_size[g_mean_lenght]{0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
void VisionCb(const prometheus_gimbal_control::VisionDiff::ConstPtr &msg)
{
    static int tmp_count = 0;
    flag_detected = msg->detect;
    g_size[tmp_count] = msg->currSize;
    tmp_count = (tmp_count + 1) % g_mean_lenght;

    float curr_size_tmp = 0;
    for (int i = 0; i < g_mean_lenght; ++i)
    {
        if (g_size[i] == 0 || (i + 1) == g_mean_lenght)
        {
            g_curr_size = curr_size_tmp / (i + 1);
            break;
        }
        curr_size_tmp += g_size[i];
    }
    g_curr_bbox_id = msg->Id;
}

float gimbal_roll, gimbal_pitch, gimbal_yaw;
void GimbalStateCb(const prometheus_gimbal_control::GimbalState::ConstPtr &msg)
{
    // g_gimbal_state = *msg;
    gimbal_roll = msg->imuAngle[0];
    gimbal_pitch = msg->imuAngle[1];
    gimbal_yaw = msg->rotorAngle[2];
}

inline float clamp(float value, float max)
{
    max = std::abs(max);
    return std::fmin(std::fmax(value, -max), max);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "circlex_land");
    ros::NodeHandle nh("~");

    // 获取 无人机ENU下的位置
    ros::Subscriber curr_pos_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, droneStateCb);

    // 获取吊舱状态
    ros::Subscriber gimbal_state_sub = nh.subscribe<prometheus_gimbal_control::GimbalState>("/gimbal/state", 10, GimbalStateCb);

    // 获取视觉反馈
    ros::Subscriber vision_sub = nh.subscribe<prometheus_gimbal_control::VisionDiff>("/gimbal/track", 10, VisionCb);

    // 【发布】发送给position_control.cpp的命令
    ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);

    float kp_x, kp_z, kp_gimbal, max_velocity, max_yaw_rate, expect_height;
    nh.param<float>("kp_x", kp_x, 100);
    nh.param<float>("kp_z", kp_z, 0.005);
    nh.param<float>("kp_gimbal", kp_gimbal, 0.01);
    nh.param<float>("max_velocity", max_velocity, 1);
    nh.param<float>("max_yaw_rate", max_yaw_rate, 10);

    ros::Rate rate(25);

    prometheus_msgs::ControlCommand comm;
    comm.Command_ID = 0;
    comm.source = "object_track";
    comm.Mode = prometheus_msgs::ControlCommand::Move;
    comm.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
    comm.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;
    comm.Reference_State.Yaw_Rate_Mode = true;
    float expect_size = 0;

    while (ros::ok())
    {
        ros::spinOnce();
        std::stringstream ss;

        if (g_curr_bbox_id != g_past_bbox_id)
        {
            int count = 0;
            g_past_bbox_id = g_curr_bbox_id;
            std::chrono::system_clock::time_point past_ts = std::chrono::system_clock::now();
            // 等待SiamRPN反馈的框稳定，KCF无此类问题
            PCOUT(0, YELLOW, "Optimizing parameters, taking 4 seconds");
            while (true)
            {
                std::chrono::system_clock::time_point now_ts = std::chrono::system_clock::now();
                auto dt = std::chrono::duration_cast<std::chrono::seconds>(now_ts - past_ts).count();
                // 只要前四妙数据
                if (dt > 4)
                    break;
                // 不要2秒数据
                if (dt > 2)
                {
                    expect_size += g_curr_size;
                    count++;
                }
                char msg[256];
                PCOUT(-1, YELLOW, msg);
                sprintf(msg, "remaining time %ds", 4 - dt);
                ros::spinOnce();
                rate.sleep();
            }
            PCOUT(0, YELLOW, "Complete optimization parameters");
            // 清空历史数据的数据
            for (int i = 0; i < g_mean_lenght; ++i)
                g_size[i] = 0;
            expect_size = expect_size / count;
        }

        if (curr_mode != "OFFBOARD")
        {
            ss << "Waiting for enable OFFBOARD... \n";
            expect_height = curr_pos[2];
        }

        if (flag_detected == 0)
        {
            comm.Reference_State.velocity_ref[0] = 0;
            comm.Reference_State.velocity_ref[1] = 0;
            comm.Reference_State.velocity_ref[2] = 0;
            comm.Reference_State.yaw_rate_ref = 0;
            PCOUT(-1, YELLOW, "Waiting for Capture target ! ");
        }
        else
        {
            // float x_vel = (expect_pitch - gimbal_pitch) *
            float x_vel = kp_x * (expect_size - g_curr_size);
            float y_vel = 0;
            float z_vel = kp_z * (expect_height - curr_pos[2]);
            float yaw_rate = -kp_gimbal * gimbal_yaw;
            comm.Reference_State.velocity_ref[0] = clamp(x_vel, max_velocity);
            comm.Reference_State.velocity_ref[1] = clamp(y_vel, max_velocity);
            comm.Reference_State.velocity_ref[2] = clamp(z_vel, max_velocity);
            comm.Reference_State.yaw_rate_ref = clamp(yaw_rate, max_yaw_rate) * M_PI / 180.;

            // 打印当前给于无人机的命令
            ss << "max velocity: " << max_velocity << " [m/s]\n"
               << "x_velocity: " << x_vel << " [m/s] "
               << "y_velocity: " << y_vel << " [m/s] "
               << "z_velocity: " << z_vel << " [m/s]\n"
               << "yaw_rate: " << yaw_rate << " [ang/s] "
               << "max yaw rate: " << max_yaw_rate << " [ang/s] \n"
               << "current pitch: " << gimbal_pitch << " [ang] \n"
               << "expect height: " << expect_height << " [m] "
               << "current height: " << curr_pos[2] << " [m]\n"
               << "expect size: " << expect_size << " [m] "
               << "current size: " << g_curr_size << " [m]\n"
               << "target distance estimate: " << curr_pos[2] * std::tan(gimbal_pitch * M_PI / 180.) << " m\n";
            PCOUT(1, GREEN, ss.str());
        }
        ++comm.Command_ID;
        command_pub.publish(comm);

        rate.sleep();
    }
}
